// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org


#include "OpenMeca/Geom/Matrix.hpp"
#include "OpenMeca/Geom/Frame.hpp"
#include "OpenMeca/Geom/Quaternion.hpp"
#include "OpenMeca/Geom/Point.hpp"

namespace OpenMeca
{
  namespace Geom
  {

    Matrix<_3D>::Matrix(const Matrix<_3D>& M, boost::function<const Frame<_3D>& ()> f):
      c0_(f), c1_(f), c2_(f)
    {
      c_[0] = &c0_;
      c_[1] = &c1_;
      c_[2] = &c2_;
      Quaternion<_3D> q(M.GetFrame().GetQuaternion(),f);
      Matrix<_3D> B =q.ToRotationMatrix();
      *this = (B*M)*B.GetInverse();
    }
    
  }
}



